#include <gio/gio.h>
#include <glib.h>
#include <sys/time.h>

#include "object.h"

#include "iec60870_slave.h"
#include "iec60870_common.h"
#include "cs104_slave.h"
#include "hal_thread.h"
#include "hal_time.h"

#define TMP_PROTOCOL_BOOST 1
#define ENABLE_ELECTRICITY 0
#define ENABLE_SEND_SINGLE_POINT_TWICE 1

static int iec104_break(void *push, void *user_data)
{
  gint64 id = 0, timestamp = 0, idx = 1;
	gint rc = 0, sum_single = 0, sum_measured = 0, qos = 0, sum_single1 = 0,
       type = CS101_COT_SPONTANEOUS;
  gboolean update = FALSE, is_sequence = FALSE, status = FALSE;
	float value_float = 0;
	CS104_Slave slave = NULL;
	CS101_ASDU asdu_single = NULL, asdu_measured = NULL;
	InformationObject io_single = NULL, io_measured = NULL;
  gpointer appLayerParameters = NULL;
#if ENABLE_SEND_SINGLE_POINT_TWICE
	CS101_ASDU asdu_single1 = NULL;
	InformationObject io_single1 = NULL;
#endif
	struct sCP56Time2a newTime;
  GObject *p = NULL, *next = NULL;
  GObject *interface;
  GObject *device;

  slave = push;
  p = user_data;
  g_object_get(p, "parent", &interface, NULL);
  g_object_get(interface, "parent", &device, NULL);
  char *clientID;
  int ca;
  g_object_get(device, "clientID", &clientID, NULL);
  ca = atoi(clientID);
  free(clientID);
	appLayerParameters = CS104_Slave_getAppLayerParameters(slave);
	do {
    g_object_get(p, "next", &next, "id", &id, "value", &value_float,
        "status", &status, "update", &update, "timestamp", &timestamp, NULL);
//    g_debug("[%"G_GINT64_FORMAT"]%f value:%s status:%d", id, value_float,
 //       value, status);
    CP56Time2a_createFromMsTimestamp(&newTime,(timestamp + 8*60*60)*1000);
		if (id < 1) {
			g_warning("point id_104 is null:%"G_GINT64_FORMAT, id);
			continue;
		}
		if (!status) {
      value_float = 0;
		}
    qos = IEC60870_QUALITY_GOOD;
//    g_debug("[%"G_GINT64_FORMAT"]%f value:%s status:%d", id, value_float,
 //       value, status);
    if (!update) {
      continue;
    }
    g_object_set(p, "update", FALSE, NULL);
	//	g_debug("ca:%d id:%"G_GINT64_FORMAT, ca, id);
		if (id < 0x4001) {//single
			if (!asdu_single) {
				asdu_single = CS101_ASDU_create(appLayerParameters, is_sequence,
            type, 0, 1, false, false);
				CS101_ASDU_setCA(asdu_single, ca);
			}
#if ENABLE_SEND_SINGLE_POINT_TWICE
      if (!asdu_single1) {
        asdu_single1 = CS101_ASDU_create(appLayerParameters, is_sequence,
            type, 0, 1, false, false);
				CS101_ASDU_setCA(asdu_single1, ca);
      }
      io_single1 =(InformationObject) SinglePointInformation_create(
         (SinglePointInformation) io_single1, id,(bool)value_float, qos);
      CS101_ASDU_addInformationObject(asdu_single1,	io_single1);
      sum_single1++;
#endif
      io_single =(InformationObject) SinglePointWithCP56Time2a_create(
         (SinglePointWithCP56Time2a) io_single, id,	(bool)value_float,
          qos, &newTime);
			rc = CS101_ASDU_addInformationObject(asdu_single,	io_single);
      idx++; sum_single++;
#if ENABLE_SEND_SINGLE_POINT_TWICE
			if (sum_single1 &&(sum_single1%20 == 0)) {
				InformationObject_destroy(io_single1);
        CS104_Slave_enqueueASDU(slave, asdu_single1);
        CS101_ASDU_destroy(asdu_single1);
				io_single1 = NULL; asdu_single1 = NULL;
			}
#endif
			if (sum_single &&(sum_single%20 == 0)) {
				InformationObject_destroy(io_single);
        CS104_Slave_enqueueASDU(slave, asdu_single);
        CS101_ASDU_destroy(asdu_single);
				io_single = NULL; asdu_single = NULL;
			}
		} else {//measured
#if ENABLE_SEND_SINGLE_POINT_TWICE
      if (asdu_single1) {
				InformationObject_destroy(io_single1);
        CS104_Slave_enqueueASDU(slave, asdu_single1);
        CS101_ASDU_destroy(asdu_single1);
				io_single1 = NULL; asdu_single1 = NULL;
      }
#endif
      if (asdu_single) {
        CS104_Slave_enqueueASDU(slave, asdu_single);
        CS101_ASDU_destroy(asdu_single);
				io_single = NULL; asdu_single = NULL;
      }
			if (!asdu_measured) {
				asdu_measured = CS101_ASDU_create(appLayerParameters, is_sequence,
            type, 0, 1, false, false);
				CS101_ASDU_setCA(asdu_measured, ca);
			}
      if (idx < 0x4001)
        idx = 0x4001;
      if ((idx != id) &&(type == CS101_COT_INTERROGATED_BY_STATION)) {
        io_measured =(InformationObject) MeasuredValueShort_create(
           (MeasuredValueShort)io_measured, idx, 0, qos);
        next = p;
      } else {
        io_measured =(InformationObject) MeasuredValueShort_create(
           (MeasuredValueShort)io_measured, id, value_float, qos);
      }
			rc = CS101_ASDU_addInformationObject(asdu_measured, io_measured);
      idx++; sum_measured++;
			if (sum_measured &&(sum_measured%20 == 0)) {
				InformationObject_destroy(io_measured);
        CS104_Slave_enqueueASDU(slave, asdu_measured);
        CS101_ASDU_destroy(asdu_measured);
				io_measured = NULL; asdu_measured = NULL;
			}
    }
	} while ((p = next));
#if ENABLE_SEND_SINGLE_POINT_TWICE
	if (io_single1) {
		InformationObject_destroy(io_single1);
	}
	if (asdu_single1) {
    CS104_Slave_enqueueASDU(slave, asdu_single1);
		CS101_ASDU_destroy(asdu_single1);
	}
#endif
	if (io_single) {
		InformationObject_destroy(io_single);
	}
	if (asdu_single) {
    CS104_Slave_enqueueASDU(slave, asdu_single);
		CS101_ASDU_destroy(asdu_single);
	}
	if (io_measured)
		InformationObject_destroy(io_measured);
	if (asdu_measured) {
    CS104_Slave_enqueueASDU(slave, asdu_measured);
		CS101_ASDU_destroy(asdu_measured);
	}
	return rc;
}
static int iec104_all(void *connection, void *user_data)
{
  gint64 id = 0, timestamp = 0, idx = 1;
	gint rc = 0, sum_single = 0, sum_measured = 0, qos = 0,
       type = CS101_COT_INTERROGATED_BY_STATION;
  gboolean update = FALSE, is_sequence = FALSE, status = FALSE;
	float value_float = 0;
	CS101_ASDU asdu_single = NULL, asdu_measured = NULL;
	InformationObject io_single = NULL, io_measured = NULL;
  gpointer appLayerParameters = NULL;
	struct sCP56Time2a newTime;
  GObject *p = NULL, *next = NULL;
  GObject *interface;
  GObject *device;

  p = user_data;
  g_object_get(p, "parent", &interface, NULL);
  g_object_get(interface, "parent", &device, NULL);
  char *clientID;
  int ca = 1;
  g_object_get(device, "clientID", &clientID, NULL);
  if (clientID) {
    ca = atoi(clientID);
    free(clientID);
  }
  appLayerParameters = IMasterConnection_getApplicationLayerParameters(connection);
	do {
    g_object_get(p, "next", &next, "id", &id, "value", &value_float,
        "status", &status, "update", &update, "timestamp", &timestamp, NULL);
//    g_debug("[%"G_GINT64_FORMAT"]%f value:%s status:%d", id, value_float,
 //       value, status);
    CP56Time2a_createFromMsTimestamp(&newTime,(timestamp + 8*60*60)*1000);
		if (id < 1) {
			g_warning("point id_104 is null:%"G_GINT64_FORMAT, id);
			continue;
		}
		if (!status) {
      value_float = 0;
		}
    qos = IEC60870_QUALITY_GOOD;
//    g_debug("[%"G_GINT64_FORMAT"]%f value:%s status:%d", id, value_float,
 //       value, status);
    is_sequence = TRUE;
    g_object_set(p, "update", FALSE, NULL);
	//	g_debug("ca:%d id:%"G_GINT64_FORMAT, ca, id);
		if (id < 0x4001) {//single
			if (!asdu_single) {
				asdu_single = CS101_ASDU_create(appLayerParameters, is_sequence,
            type, 0, 1, false, false);
				CS101_ASDU_setCA(asdu_single, ca);
			}
      if (id != idx) {
        io_single =(InformationObject) SinglePointInformation_create(
           (SinglePointInformation) io_single, idx, false, qos);
        next = p;
      } else {
        io_single =(InformationObject) SinglePointInformation_create(
           (SinglePointInformation) io_single, id,	(bool)value_float, qos);
      }
			rc = CS101_ASDU_addInformationObject(asdu_single,	io_single);
      idx++; sum_single++;
			if (sum_single &&(sum_single%20 == 0)) {
				InformationObject_destroy(io_single);
        IMasterConnection_sendASDU(connection, asdu_single);
        CS101_ASDU_destroy(asdu_single);
				io_single = NULL; asdu_single = NULL;
			}
		} else {//measured
      if (asdu_single) {
        IMasterConnection_sendASDU(connection, asdu_single);
        CS101_ASDU_destroy(asdu_single);
				io_single = NULL; asdu_single = NULL;
      }
			if (!asdu_measured) {
				asdu_measured = CS101_ASDU_create(appLayerParameters, is_sequence,
            type, 0, 1, false, false);
				CS101_ASDU_setCA(asdu_measured, ca);
			}
      if (idx < 0x4001)
        idx = 0x4001;
      if ((idx != id) &&(type == CS101_COT_INTERROGATED_BY_STATION)) {
        io_measured =(InformationObject) MeasuredValueShort_create(
           (MeasuredValueShort)io_measured, idx, 0, qos);
        next = p;
      } else {
        io_measured =(InformationObject) MeasuredValueShort_create(
           (MeasuredValueShort)io_measured, id, value_float, qos);
      }
			rc = CS101_ASDU_addInformationObject(asdu_measured, io_measured);
      idx++; sum_measured++;
			if (sum_measured &&(sum_measured%20 == 0)) {
				InformationObject_destroy(io_measured);
        IMasterConnection_sendASDU(connection, asdu_measured);
        CS101_ASDU_destroy(asdu_measured);
				io_measured = NULL; asdu_measured = NULL;
			}
    }
	} while ((p = next));
	if (io_single) {
		InformationObject_destroy(io_single);
	}
	if (asdu_single) {
    IMasterConnection_sendASDU(connection, asdu_single);
		CS101_ASDU_destroy(asdu_single);
	}
	if (io_measured)
		InformationObject_destroy(io_measured);
	if (asdu_measured) {
    IMasterConnection_sendASDU(connection, asdu_measured);
		CS101_ASDU_destroy(asdu_measured);
	}
	return rc;
}
static bool
clockSyncHandler(void* parameter, IMasterConnection connection, CS101_ASDU asdu, CP56Time2a newTime)
{
	uint64_t now_time = 0, new_time = 0;

	g_debug("Process time sync command with time \n"); 
	now_time = g_get_real_time();
	new_time = CP56Time2a_toMsTimestamp(newTime);
	if (labs(new_time - now_time) > 60*1000) {
    /*
    struct timeval tv;
    tv.tv_sec =(new_time/1000) - 8*60*60;
    tv.tv_usec = 0;
    int result = settimeofday(&tv, NULL);
    if (result == 0) {
        g_warning("System time set successfully.");
    } else {
        g_warning("Error setting system time.:%"G_GUINT64_FORMAT" :%s",
            new_time, strerror(errno));
    }
    */
    GDateTime *dt = NULL;
    char *time_str = NULL, buf[64];
    dt = g_date_time_new_from_unix_local(new_time/1000 - 8*60*60);
    time_str = g_date_time_format (dt, "%Y-%m-%d %H:%M:%S");
    snprintf(buf, sizeof(buf), "sudo date -s \"%s\"", time_str);
    system(buf);
    system("sudo hwclock --systohc");
    if (dt)
      g_date_time_unref(dt);
    if (time_str)
      g_free(time_str);
	}
	return true;
}
static bool counter_interrogation_handler(void* parameter,
		IMasterConnection connection,
		CS101_ASDU asdu, QualifierOfCIC qcc)
{
	if (qcc == 5) {
		IMasterConnection_sendACT_CON(connection, asdu, false);
		IMasterConnection_sendACT_TERM(connection, asdu);
	}
	return true;
}
static bool
interrogationHandler(void* parameter, IMasterConnection connection, CS101_ASDU asdu, uint8_t qoi)
{
	g_debug("L%d f-%s Received interrogation for group %i\n",
      __LINE__, __func__, qoi);

	if (qoi == 20) { /* only handle device interrogation */
		IMasterConnection_sendACT_CON(connection, asdu, false);
		iec104_all(connection, parameter);
		IMasterConnection_sendACT_TERM(connection, asdu);
	} else {
		IMasterConnection_sendACT_CON(connection, asdu, true);
	}

	return true;
}

static bool
asduHandler(void* parameter, IMasterConnection connection, CS101_ASDU asdu)
{
	int rc = false;

	if (CS101_ASDU_getTypeID(asdu) == C_SC_NA_1) {
		g_debug("received single command\n");

		if (CS101_ASDU_getCOT(asdu) == CS101_COT_ACTIVATION) {
			InformationObject io = CS101_ASDU_getElement(asdu, 0);

			if (InformationObject_getObjectAddress(io) == 5000) {
				SingleCommand sc =(SingleCommand) io;

				g_debug("IOA: %i switch to %i\n", InformationObject_getObjectAddress(io),
					SingleCommand_getState(sc));

				CS101_ASDU_setCOT(asdu, CS101_COT_ACTIVATION_CON);
			} else
				CS101_ASDU_setCOT(asdu, CS101_COT_UNKNOWN_IOA);

			InformationObject_destroy(io);
			IMasterConnection_sendACT_TERM(connection, asdu);
		} else
			CS101_ASDU_setCOT(asdu, CS101_COT_UNKNOWN_COT);
		IMasterConnection_sendASDU(connection, asdu);
		rc = true;
		goto err;
	}	
err:
	return rc;
}

static bool connectionRequestHandler(void* parameter, const char* ipAddress)
{
	g_debug("New connection from %s\n", ipAddress);
	return true;
}
void connectionEventHandler(void* parameter, IMasterConnection con, CS104_PeerConnectionEvent event)
{
	//int port = 0;

	if (event == CS104_CON_EVENT_CONNECTION_CLOSED) {
	//	g_debug("[%d]disconnect connection from\n", port);
	}
}
static void rawMessageHandler(void* parameter, IMasterConnection con,
		uint8_t* msg, int msgSize, bool sent)
{
	int port = 0;

	if (sent)
		printf("[%d]----: ", port);
	else
		printf("[%d]++++: ", port);

	int i;
	for(i = 0; i < msgSize; i++) {
		printf("%02x ", msg[i]);
	}

	printf("\n");
}
void *iec104_slave_init(int port, void *user_data)
{

	if (port < 1)
		port = 2404;
	CS104_Slave slave = CS104_Slave_create(100, 20000);
	CS104_Slave_setLocalAddress(slave, "0.0.0.0");
	CS104_Slave_setLocalPort(slave, port);
	CS104_Slave_setServerMode(slave,
			CS104_MODE_CONNECTION_IS_REDUNDANCY_GROUP);
	CS104_Slave_setClockSyncHandler(slave, clockSyncHandler, NULL);
	CS104_Slave_setInterrogationHandler(slave, interrogationHandler, user_data);
	CS104_Slave_setASDUHandler(slave, asduHandler, user_data);
	CS104_Slave_setCounterInterrogationHandler(slave,
			counter_interrogation_handler, user_data);
	CS104_Slave_setConnectionRequestHandler(slave,
			connectionRequestHandler, NULL);
	CS104_Slave_setConnectionEventHandler(slave,
			connectionEventHandler, &port);
	CS104_Slave_setRawMessageHandler(slave, rawMessageHandler, &port);
	CS104_Slave_start(slave);
	g_debug("slave:%p, port:%d\n", slave, port);
	if (CS104_Slave_isRunning(slave) == false) {
		g_debug("Starting server failed!\n");
	}
  return slave;
}
void iec104_slave_run(void *slave, void *user_data)
{
  gint64 tm0 = 0, tm1 = 0;
	int openConnections = 0;

  tm0 = g_get_real_time()/(1000*1000);
	while (1) {
		int connectionsCount = CS104_Slave_getOpenConnections(slave);
		if (connectionsCount != openConnections) {
			openConnections = connectionsCount;
			g_debug("Connected clients: %i\n", openConnections);
		}
		if (connectionsCount) {
      tm1 = g_get_real_time()/(1000*1000);
      if ((tm1 < tm0) || ((tm1 - tm0) > 1)) {
        tm0 = tm1;
        iec104_break(slave, user_data);
      }
    }
		Thread_sleep(1000);
	}
}
void iec104_slave_unref(void *slave)
{
	CS104_Slave_stop(slave);
	CS104_Slave_destroy(slave);
}
